"""
名称：CobotGPT_APF部署测试(虚拟机)
功能：测试需要收集什么数据，用来训练碰撞检测模型
缺陷：
"""

from robotcontrol import *
# import csv
import time
import timeit
from multiprocessing import Process, Queue
# from kinectcontrol import *
# import random
from math import pi

from User.Releases.Artifial_potential_field_RRT import APF_RRT,get_obstacle
from Modules.robot_class import *

import numpy as np
import torch
import torch.nn as nn

def Link_Transformation(last_i, i, a_list, alpha_list, d_list, theta_list):
    """
    * function：坐标系{i-1}到坐标系{i}的转换矩阵
    * tips：这里的last_i指的是i-1
    """
    i = i  # 下面使用的i-1表示列表的第i-1个数，注意同DH参数里的i-1区别
    T_martix = np.mat(np.zeros((4, 4)))

    T_martix[0, 0] = np.cos(theta_list[i - 1])
    T_martix[0, 1] = -1 * np.sin(theta_list[i - 1])
    T_martix[0, 2] = 0
    T_martix[0, 3] = a_list[i - 1]

    T_martix[1, 0] = np.sin(theta_list[i - 1]) * np.cos(alpha_list[i - 1])
    T_martix[1, 1] = np.cos(theta_list[i - 1]) * np.cos(alpha_list[i - 1])
    T_martix[1, 2] = -1 * np.sin(alpha_list[i - 1])
    T_martix[1, 3] = -1 * np.sin(alpha_list[i - 1]) * d_list[i - 1]

    T_martix[2, 0] = np.sin(theta_list[i - 1]) * np.sin(alpha_list[i - 1])
    T_martix[2, 1] = np.cos(theta_list[i - 1]) * np.sin(alpha_list[i - 1])
    T_martix[2, 2] = np.cos(alpha_list[i - 1])
    T_martix[2, 3] = np.cos(alpha_list[i - 1]) * d_list[i - 1]

    T_martix[3, 0] = 0
    T_martix[3, 1] = 0
    T_martix[3, 2] = 0
    T_martix[3, 3] = 1

    return T_martix


def get_robot_position(joint_radian: tuple = (0, 0, 0, 0, 0, 0)):
    """
    * 功能：机器人正解，获取六个关节坐标
    * 输入：机械臂关节角
    * 输出：六个关节坐标：6*3=18，类型tuple，单位m
    """
    # 初始化参数（DH参数）
    a_list = [0, 0, 408, 376, 0, 0]
    alpha_list = [0, np.pi / 2, np.pi, np.pi, np.pi / 2, np.pi / 2]
    d_list = [98.5, 121.5, 0, 0, 102.5, 94]
    # theta_list = [0, np.pi / 2, 0, np.pi / 2, np.pi, 0]  # 输入想要转动的角度（此处设置转动的角度在转动后即达到机械臂伸直状态）
    theta_offset = [0, np.pi / 2, 0, np.pi / 2, np.pi, 0]  # 关节角补偿
    # print(theta_offset)
    joint_radian = list(joint_radian)
    # print(joint_radian)
    theta_list = [a + b for a, b in zip(joint_radian, theta_offset)]  # 机械臂关节角：正转+，反转-
    # print(theta_list)

    # 例：T_0_1表示坐标系{0}到坐标系{1}的转换矩阵
    T_0_1 = Link_Transformation(0, 1, a_list, alpha_list, d_list, theta_list)
    T_1_2 = Link_Transformation(1, 2, a_list, alpha_list, d_list, theta_list)
    T_2_3 = Link_Transformation(2, 3, a_list, alpha_list, d_list, theta_list)
    T_3_4 = Link_Transformation(3, 4, a_list, alpha_list, d_list, theta_list)
    T_4_5 = Link_Transformation(4, 5, a_list, alpha_list, d_list, theta_list)
    T_5_6 = Link_Transformation(5, 6, a_list, alpha_list, d_list, theta_list)

    T_0_6 = T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5 * T_5_6  # 二维数组，形状(4,4)，即矩阵
    T_0_5 = T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5
    T_0_4 = T_0_1 * T_1_2 * T_2_3 * T_3_4
    T_0_3 = T_0_1 * T_1_2 * T_2_3
    T_0_2 = T_0_1 * T_1_2
    T_0_1 = T_0_1
    # print(type(T_0_6))    # 查看数组形状：(4,4)
    # T_0_6 = np.array(T_0_6)
    # print(type(T_0_6))

    # 机器人关节坐标
    joint1_pos = T_0_1[0:3, 3].copy()
    joint2_pos = T_0_2[0:3, 3].copy()
    joint3_pos = T_0_3[0:3, 3].copy()
    joint4_pos = T_0_4[0:3, 3].copy()
    joint5_pos = T_0_5[0:3, 3].copy()
    joint6_pos = T_0_6[0:3, 3].copy()
    # print("joint1_pos: ",joint1_pos)
    # print("joint2_pos: ",joint2_pos)
    # print("joint3_pos: ",joint3_pos)
    # print("joint4_pos: ",joint4_pos)
    # print("joint5_pos: ",joint5_pos)
    # print("joint6_pos: ",joint6_pos)

    # <class 'numpy.matrix'> 转换为 <class 'numpy.ndarray'>
    arr1 = np.array(joint1_pos)
    arr2 = np.array(joint2_pos)
    arr3 = np.array(joint3_pos)
    arr4 = np.array(joint4_pos)
    arr5 = np.array(joint5_pos)
    arr6 = np.array(joint6_pos)
    # print(type(arr1))
    # print(arr6)
    # arr1 = arr1.astype(float)
    # arr2 = arr2.astype(float)
    # arr3 = arr3.astype(float)
    # arr4 = arr4.astype(float)
    # arr5 = arr5.astype(float)
    # arr6 = arr6.astype(float)

    # 使用flatten()方法将矩阵降级为一维数组
    flat_array_1 = arr1.flatten() / 1000.0
    flat_array_2 = arr2.flatten() / 1000.0
    flat_array_3 = arr3.flatten() / 1000.0
    flat_array_4 = arr4.flatten() / 1000.0
    flat_array_5 = arr5.flatten() / 1000.0
    flat_array_6 = arr6.flatten() / 1000.0
    # print(flat_array_1)
    # print(flat_array_2)
    # print(flat_array_3)
    # print(flat_array_4)
    # print(flat_array_5)
    # print(flat_array_6)

    # 使用tuple()函数将一维数组转换为元组
    # 改为list
    tuple1_arr = list(flat_array_1)
    tuple2_arr = list(flat_array_2)
    tuple3_arr = list(flat_array_3)
    tuple4_arr = list(flat_array_4)
    tuple5_arr = list(flat_array_5)
    tuple6_arr = list(flat_array_6)
    # print(tuple1_arr)
    # print(tuple2_arr)
    # print(tuple3_arr)
    # print(tuple4_arr)
    # print(tuple5_arr)
    # print(tuple6_arr)
    tuple_all = tuple1_arr + tuple2_arr + tuple3_arr + tuple4_arr + tuple5_arr + tuple6_arr
    # print(tuple_all)
    # print(type(tuple_all))

    joint_position = tuple_all
    # print(T_0_6)
    return joint_position


def delay(microseconds):
    """
    功能：延迟xx微秒，1s = 1*10^3 ms = 1*10^6 us = 1*10^9 ns
    输入：
    输出
    """
    start = timeit.default_timer()
    while True:
        elapsed = (timeit.default_timer() - start) * 10 ** 6
        if elapsed >= microseconds:
            break
    return 0


class CobotGPT:

    def __init__(self):
        """
        功能：连接机械臂
        输入：
        输出：连接状态码
        """

        # 链接服务器
        # self.ip = '192.168.213.128'
        self.ip = '192.168.199.128'  # 注意配置网线口的适配器属性：关掉IPV6，手动设置IPV4地址和机械臂在同一个网段
        # self.ip = '192.168.177.128'  # 注意配置网线口的适配器属性：关掉IPV6，手动设置IPV4地址和机械臂在同一个网段
        self.port = 8899
        self.joint_radian_list = [(80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                                  (
                                  160 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180,
                                   0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180,
                                   0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 0 * pi / 180,
                                   0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180),  # 第一阶段
                                  (160 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180),
                                  (90 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180),
                                  (90 * pi / 180, -20 * pi / 180, 80 * pi / 180, 10 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180), ]

    def robot_set_position(self, cnt, queue1: Queue, queue2: Queue):
        """
        功能：设置机械臂的固定轨迹点，并实时获取关节角信息
        输入：cnt测试次数，queue1是否碰撞，queue2机械臂关节角
        输出：
        """

        # 初始化logger
        logger_init()
        # 启动测试
        logger.info("{0} robot_set_position() running...".format(Auboi5Robot.get_local_time()))
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                # 上电
                robot.robot_startup()
                # 设置碰撞等级
                robot.set_collision_class(10)
                # 初始化全局配置文件
                robot.init_profile()
                # 循环测试

                # 设置关节最大加速度
                robot.set_joint_maxacc((1.5, 1.5, 1.5, 1.5, 1.5, 1.5))
                # 设置关节最大速度
                robot.set_joint_maxvelc((0.5, 0.5, 0.5, 0.5, 0.5, 0.5))

                while 1:
                    # while queue2.empty() is not True:
                    #     queue2.get()
                    # path = queue2.get()
                    # # print(path)
                    # path_list = list()
                    # for i in range(len(path)):
                    #     path_list.append(path[i].theta)
                    # # print("path_list:", path_list)
                    # while 1:
                    #     for i in range(len(path_list)):
                    #         path = tuple(path_list[i])
                    #         logger.info("move joint to {0}".format(path))
                    #         robot.move_joint(path)
                    #     time.sleep(2)
                    #     ori = (55 * pi / 180, -6 * pi / 180, 26 * pi / 180, 0 * pi / 180, 30 * pi / 180, -54 * pi / 180)
                    #     logger.info("move joint to {0}".format(ori))
                    #     robot.move_joint(ori)
                    for i in range(len(self.joint_radian_list)):
                        joint_radian = self.joint_radian_list[i]
                        logger.info("move joint to {0}".format(joint_radian))
                        robot.move_joint(joint_radian)



        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in robot_set_position()".format(robot.get_local_time(), e))

        # 无论是否有异常，都会执行的代码
        finally:
            # 断开服务器链接
            # if robot.connected:
            #     # 关闭机械臂
            #     robot.robot_shutdown()
            #     # 断开机械臂链接
            #     robot.disconnect()
            # # 释放库资源
            # Auboi5Robot.uninitialize()
            logger.info("{0} test completed.".format(Auboi5Robot.get_local_time()))





    def collision_detection(self, queue1: Queue, queue2: Queue):

        collision_flag = 0  # 默认未发生碰撞
        """
                功能：设置机械臂的固定轨迹点，并实时获取关节角信息
                输入：cnt测试次数，queue1是否碰撞，queue2机械臂关节角
                输出：
                """

        # 初始化logger
        logger_init()
        # 启动测试
        logger.info("{0} collision_detection() running...".format(Auboi5Robot.get_local_time()))

        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                # 上电
                robot.robot_startup()
                # 设置碰撞等级
                robot.set_collision_class(10)
                # 初始化全局配置文件
                robot.init_profile()

                # 设置关节最大加速度
                robot.set_joint_maxacc((1.5, 1.5, 1.5, 1.5, 1.5, 1.5))
                # 设置关节最大速度
                # robot.set_joint_maxvelc((0.2, 0.2, 0.2, 0.2, 0.2, 0.2))
                # robot.set_joint_maxvelc((0.1, 0.1, 0.1, 0.1, 0.1, 0.1))
                robot.set_joint_maxvelc((0.3, 0.3, 0.3, 0.3, 0.3, 0.3))

                # 回到初始位姿
                joint_radian_ori = (0, 0, 0, 0, 0, 0)  # 初始位姿,turple类型
                # print(type(joint_radian_ori))
                logger.info("回到初始位姿：move joint to {0}".format(joint_radian_ori))
                robot.move_joint(joint_radian_ori)

                # 设定机器人运动的起点、终点
                start = Joint_point(
                    [55 * pi / 180, -6 * pi / 180, 26 * pi / 180, 0 * pi / 180, 30 * pi / 180, -54 * pi / 180])
                goal = Joint_point(
                    [170 * pi / 180, -20 * pi / 180, 80 * pi / 180, 0 * pi / 180, 90 * pi / 180, -170 * pi / 180])
                print("start:",
                      start.theta)  # start: [0.9599310885968813, -0.10471975511965977, 0.4537856055185257, 0.0, 0.5235987755982988, -0.9424777960769379]
                print("goal:",
                      goal.theta)  # goal: [2.9670597283903604, -0.3490658503988659, 1.3962634015954636, 0.0, 1.5707963267948966, -2.9670597283903604]

                logger.info("回到star：move joint to {0}".format(start.theta))
                robot.move_joint(tuple(start.theta))

                # cube_obstacle = Cube_obstacle(Point([-0.6, -0.6, 0.15]), 0.3, 0.3, 0.3)

                # 设定机器人的关节角空间
                joint_limit_angle = [
                    Joint_point([-170 * pi / 180, -170 * pi / 180, -170 * pi / 180, -170 * pi / 180, -170 * pi / 180,
                                 -170 * pi / 180]),
                    Joint_point([170 * pi / 180, 170 * pi / 180, 170 * pi / 180, 170 * pi / 180, 170 * pi / 180,
                                 170 * pi / 180])]

                # 获取的关节角、骨骼坐标信息
                skeleton_position_list = [-0.5043224874442438, 0.22287961370105214, 0.3062320074838769,
                                          -0.5043224874442438, 0.22287961370105214, 0.3062320074838769
                    , -0.5043224874442438, 0.22287961370105214, 0.3062320074838769, -0.5043224874442438,
                                          0.22287961370105214, 0.3062320074838769,
                                          -0.5043224874442438, 0.22287961370105214, 0.3062320074838769,
                                          -0.5043224874442438, 0.22287961370105214, 0.3062320074838769
                    , -0.5043224874442438, 0.22287961370105214, 0.3062320074838769, -0.5043224874442438,
                                          0.22287961370105214, 0.3062320074838769,
                                          -0.5043224874442438, 0.22287961370105214, 0.3062320074838769, 0.24,
                                          0.44, 0.77]
                joint_position_list = [0.0, -0.0, 0.0985, 0.09015006467853738, -0.0814568342034941, 0.09850000000000002,
                                       0.13105307436421948, -0.03618857642294668, 0.5019126034671784,
                                       0.3715983664018288,
                                       0.23002817318868934, 0.38947326557888984, 0.4235396556924686, 0.287512737066289,
                                       0.45658419898491714, 0.48124881149778836, 0.3155648722781763,
                                       0.38789117862410233]

                # 先确定障碍物坐标，即找出距离机器人关节最近的骨骼点坐标，并将骨骼点坐标实例化为实体障碍物
                cube_obstacle = get_obstacle(skeleton_position_list, joint_position_list)
                print("obstacle positiopn(xyz,lwh):", cube_obstacle.x, cube_obstacle.y, cube_obstacle.z,
                      cube_obstacle.l, cube_obstacle.w, cube_obstacle.h)
                #  0.24 0.44 0.77 0.3 0.3 0.3

                # 运行人工势场法
                apf_rrt = APF_RRT(start, goal, cube_obstacle, joint_limit_angle)
                # path = apf_rrt.path_planning(queue2)

                start = time.time()
                # apf_rrt.path_planning(queue2)
                apf_rrt.path_planning()
                end = time.time()
                print("计算耗费{:.2f}s".format(end - start))




# 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in collision_detection()".format(robot.get_local_time(), e))

        # 无论是否有异常，都会执行的代码
        finally:
            # 断开服务器链接
            # if robot.connected:
            #     # 关闭机械臂
            #     robot.robot_shutdown()
            #     # 断开机械臂链接
            #     robot.disconnect()
            # # 释放库资源
            # Auboi5Robot.uninitialize()
            # logger.info("{0} test completed.".format(Auboi5Robot.get_local_time()))
            pass

    def data_set(self,):
        """
        * 功能：轨迹优化
        * 输入：queue1:输入骨骼坐标
        * 输出：机械臂下一个运动关节角，类型tuple
        """

        # 初始化logger
        logger_init()
        # 启动测试
        logger.info("Func4：{0} collision_path_optimize() running...".format(Auboi5Robot.get_local_time()))
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                # 上电
                robot.robot_startup()
                # 设置碰撞等级
                robot.set_collision_class(10)
                # 初始化全局配置文件
                robot.init_profile()

                # 设置关节最大加速度
                robot.set_joint_maxacc((1.5, 1.5, 1.5, 1.5, 1.5, 1.5))
                # 设置关节最大速度
                # robot.set_joint_maxvelc((0.2, 0.2, 0.2, 0.2, 0.2, 0.2))
                # robot.set_joint_maxvelc((0.1, 0.1, 0.1, 0.1, 0.1, 0.1))
                robot.set_joint_maxvelc((0.3, 0.3, 0.3, 0.3, 0.3, 0.3))

                while 1:
                    x = robot.get_current_waypoint()
                    y = robot.get_joint_status()

                    # 当前关节角
                    current_joint_radian = x['joint']

                    # 当前关节状态：电流，电压，温度
                    joint1_status = [y['joint1']['current'],y['joint1']['voltage'],y['joint1']['temperature']]
                    joint2_status = [y['joint2']['current'],y['joint2']['voltage'],y['joint2']['temperature']]
                    joint3_status = [y['joint3']['current'],y['joint3']['voltage'],y['joint3']['temperature']]
                    joint4_status = [y['joint4']['current'],y['joint4']['voltage'],y['joint4']['temperature']]
                    joint5_status = [y['joint5']['current'],y['joint5']['voltage'],y['joint5']['temperature']]
                    joint6_status = [y['joint6']['current'],y['joint6']['voltage'],y['joint6']['temperature']]
                    joint_status = joint1_status + joint2_status + joint3_status + joint4_status + joint5_status + joint6_status

                    # 当前关节位置
                    current_joint_position = get_robot_position(current_joint_radian)

                    print(f'current_joint_radian: {current_joint_radian}' )
                    print(f'current_joint_position: {current_joint_position}')
                    print(f'joint_status: {joint_status}')
                    time.sleep(1)

        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1}".format(robot.get_local_time(), e))

        except KeyboardInterrupt:
            # 断开服务器链接
            if robot.connected:
                # 断开机械臂链接
                robot.disconnect()
            # 释放库资源
            Auboi5Robot.uninitialize()
            print("robot disconnected-------------------------")

        # 无论是否有异常，都会执行的代码
        finally:
            # 断开服务器链接
            # if robot.connected:
            #     # 关闭机械臂
            #     robot.robot_shutdown()
            #     # 断开机械臂链接
            #     robot.disconnect()
            # # 释放库资源
            # Auboi5Robot.uninitialize()
            logger.info("{0} test completed.".format(Auboi5Robot.get_local_time()))

if __name__ == '__main__':

    cobot = CobotGPT()


    # 创建通信队列
    q1 = Queue()
    q2 = Queue(maxsize=1)

    # # 创建子进程
    p1 = Process(target=cobot.robot_set_position, kwargs={"cnt": 10, "queue1": q1, "queue2": q2})
    # p2 = Process(target=cobot.robot_get_waypoint)
    # p3 = Process(target=cobot.collision_detection, kwargs={"queue1": q1, "queue2": q2})
    # p4 = Process(target=cobot.dataset, kwargs={"queue": q3})
    p5 = Process(target=cobot.data_set)

    p1.start()
    # p2.start()
    # p3.start()
    # p4.start()
    p5.start()
    # 主进程
    # game.run(q3)
    # p1.join()
    # p2.join()
    # WAPOINT.run(ip='192.168.199.128', port=8899)
